#include "Geometry.h"

/*!
 *	@brief		Compute the smallest distance between two line segments.
 *
 *	@param		seg1		The first segment.
 *	@param		seg2		The second segmend.
 *	@returns	The distance between the closest features on the two segments.
 */
float minDistance( const Segment & seg1, const Segment & seg2 ) {
	Vector2 qDir = seg1.direction();
	float qLen = seg1.length();
	Vector2 q0 = seg1._p0;
	Vector2 q1 = seg1._p1;

	float pLen = seg2.length();
	Vector2 pDir = seg2.direction();
	Vector2 p0 = seg2._p0;
	Vector2 p1 = seg2._p1;

	if ( fabs( qDir * pDir ) >= 0.9999f ) {
		/// The two segments lie on parallel lines
		///	If one projects onto the other at all, then it is the distance between the lines
		///	otherwise, the distance between the closest end points.

		// Project p0 and p1 onto the line segment
		float t0 = qDir * ( p0 - q0 );
		float t1 = qDir * ( p1 - q0 );
		if ( t0 > qLen && t1 > qLen ) {
			// seg2 is beyond seg 1's second end point
			if ( t0 < t1 ) {	// p0 is closest to q1
				return ( p0 - q1 ).magnitude();
			} else {			// p1 is closest to q1
				return ( p1 - q1 ).magnitude();
			}
		} else if ( t0 < 0.f && t1 < 0.f ) {
			// seg2 is before seg 1's first end point
			if ( t0 > t1 ) {	// p0 is closest to q0
				return ( p0 - q0 ).magnitude();
			} else {			// p1 is closest to q0
				return ( p1 - q0 ).magnitude();
			}
		} else {
			// There is overlap - distance is distance between lines
			Vector2 test = q0 + qDir * t0;
			return ( test - p0 ).magnitude();
		}
	} else {
		/// Test for intersection

		// Where do the lines intersect
		float denom = det( qDir, pDir ); 
		float num = det( pDir, q0 - p0 );
		if ( fabs( denom ) <= 0.0001f ) {
			float altNum = det( pDir, q1 - p0 );
			if ( ( altNum >= 0.f && num <= 0.f ) ||
				( altNum <= 0.f && num >= 0.f ) ) {
				// The two end points of q lie on opposite sides of q
				return 0.f;
			}
		} else {
			// Lines converge
			float t = num / denom;
			if ( t >= 0.f && t <= qLen ) {
				Vector2 s = q0 + qDir * t;
				float r = ( s - p0 ) * pDir;
				if ( r >= 0.f && r <= pLen ) {
					// lines intersect in the segment intervals
					return 0.f;
				}
			}
		}
			
		/// first test the distances between segment end points
		float closestDist = ( p0 - q0 ).magnitude();

		float dist = ( p1 - q0).magnitude();
		closestDist = dist < closestDist ? dist : closestDist;
		dist = ( p0 - q1 ).magnitude();
		closestDist = dist < closestDist ? dist : closestDist;
		dist = ( p1 - q1 ).magnitude();
		closestDist = dist < closestDist ? dist : closestDist;

		// Now test for projections of endpoints onto segments
		Vector2 s;
		float t = qDir * ( p0 - q0 );
		if ( t >= 0 && t <= qLen ) {
			s = q0 + t * qDir;
			dist = ( p0 - s ).magnitude();
			closestDist = dist < closestDist ? dist : closestDist;
		}
		t = qDir * ( p1 - q0 );
		if ( t >= 0 && t <= qLen ) {
			s = q0 + t * qDir;
			dist = ( p1 - s ).magnitude();
			closestDist = dist < closestDist ? dist : closestDist;
		}
		t = pDir * ( q0 - p0 );
		if ( t >= 0 && t <= pLen ) {
			s = p0 + t * pDir;
			dist = ( q0 - s ).magnitude();
			closestDist = dist < closestDist ? dist : closestDist;
		}
		t = pDir * ( q1 - p0 );
		if ( t >= 0 && t <= pLen ) {
			s = p0 + t * pDir;
			dist = ( q1 - s ).magnitude();
			closestDist = dist < closestDist ? dist : closestDist;
		}
		return closestDist;
	}
}

float minDistance( const Segment & seg, const Vector2& v )
{
	// distance from point to line
	Vector2 dir = seg._p1 - seg._p0;
	Vector2 dirPerp(dir._y, -dir._x);

	Vector2 r = seg._p0 - v;

	Vector2 normalizedDirPerp = dirPerp.norm();
	Vector2 normalizedDir = dir.norm();
	float dist = abs(r * normalizedDirPerp);

	// check intersection pt is in the segment range
	float distFromS0 = -r * normalizedDir;
	if (distFromS0 < 0.0f)
	{
		dist = r.magnitude();
	}
	else if (distFromS0 > dir.magnitude())
	{
		dist = (seg._p1 - v).magnitude();
	}

	return dist;
}